#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustom9V)
#pragma config(Sensor, S3,     SMUX_1,              sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     e,             tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     a,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     b,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     c,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     f,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     d,             tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    frontarm,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    backarm,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    dispencer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    Irpullup,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    Irfront,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    Irback,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/CompleteDriver.h"

bool side;
bool balancing;
void decider() {
  bDisplayDiagnostics = false;
  wait1msec(20);
  eraseDisplay();
  nxtDisplayTextLine(1, "Right or Left?");
  side = RIGHTSD;
  nxtDisplayTextLine(2, "Right");
  while (nNxtButtonPressed != kEnterButton){
    if (nNxtButtonPressed == kRightButton){
      nxtDisplayTextLine(2, "Right");
      side = RIGHTSD;
    }
    if (nNxtButtonPressed == kLeftButton){
      nxtDisplayTextLine(2, "Left");
      side = LEFTSD;
    }
  }
  eraseDisplay();
  nxtDisplayTextLine(1, "Cross or Balance");
  balancing = true;
  nxtDisplayTextLine(2, "Balance");
  while (nNxtButtonPressed == kEnterButton);
  while (nNxtButtonPressed != kEnterButton){
    if (nNxtButtonPressed == kRightButton){
      nxtDisplayTextLine(2, "Balance");
      balancing = true;
    }
    if (nNxtButtonPressed == kLeftButton){
      nxtDisplayTextLine(2, "cross");
      balancing = false;
    }
  }
  eraseDisplay();
  nxtDisplayTextLine(1, "Done");
}

void initialize() {
  HTSMUXinit();
  HTSMUXscanPorts(SMUX_1);
  HTSMUXscanPorts(SMUX_2);
  wait1Msec(100);
  kalmindebugdisplay = false;
  bDisplayDiagnostics = false;
  StartTask(sensorread);
  calibrateAccel();
  eraseDisplay();
  decider();
  startkalmin();
  wait1Msec(2000);
  servo[frontarm] = SERVO_FRONTARM_UP;
  servo[backarm] = 244;
  servo[dispencer] = 5;
  allignHeight (RETRIEVING_MIDDLE);
  bDisplayDiagnostics = true;
}

task main {
  //eraseDisplay();
  //StartTask(sensorread);
  initialize();
  //bDisplayDiagnostics = false;
  //while (true){
 //   showValues();
 // }
  waitForStart();
  if (side == RIGHTSD){
    adjustedDAS(315, 4100, 30);
  }
  else {
    adjustedDAS(45, 4100, 30);
  }
  adjustedDAS(0, 200, 30);
  adjustedDAS(180, 200, 30);
  servo[frontarm] = SERVO_FRONTARM_DOWN;
  wait1Msec(1000);
  adjustedDAS(0, 500, 30);
  if (balancing){
    balance();
  }
  else {
    crossbridge(true, 7000, 30, true);
    wait1Msec(2000);
    adjustedDAS(0, 1000, 30);
  }
}
